#include <iostream>
#include <opencv2/opencv.hpp>
#include <elas.h>
#include <image.h>
#include <stdio.h>

using namespace std;
using namespace cv;

int main()
{
	Mat leftImage = imread("/home/wenyi/workspace/stereoImage/left_down.jpg", IMREAD_GRAYSCALE);
	Mat rightImage = imread("/home/wenyi/workspace/stereoImage/right_down.jpg", IMREAD_GRAYSCALE);

	if(leftImage.size() != rightImage.size())
	{
		cout << "error: the image size of the left and right is not equal!!!" << endl;
		return -1;
	}

	float fx = 481.071198;
	float T = 79.615486;
	float depthFact = T * fx * 1000.0f;

	int width = leftImage.cols;
	int height = leftImage.rows;
	int dim[3] = {width, height, width};

	Mat dispLeft = Mat::zeros(Size(width, height), CV_32FC1);
	Mat dispRight = Mat::zeros(Size(width, height), CV_32FC1);

#if 1  //correct image

	//down parameters
	Mat leftIntrinsics = (Mat_<double>(3,3) << 481.071198, 0., 319.511902,
												0., 481.071198, 237.524200,
												0., 0., 1.0);

	Mat rightIntrinsics = (Mat_<double>(3,3) << 481.071198, 0., 319.511902,
												0., 481.071198, 237.524200,
												0., 0., 1.0);

	Mat rotationLeftInRight = (Mat_<double>(3,3) << 1.0, 0., 0.,
													0., 1.0, 0.,
													0., 0., 1.0);

	Mat translationLeftInRight = (Mat_<double>(3,1) << -79.615486, 0., 0.0);

	Mat discoff = (Mat_<double>(5,1) << 0., 0., 0., 0., 0.);

	Mat rL, rR, pL, pR;
	Mat mapLx, mapLy, mapRx, mapRy;

	Mat rectifyImageL, rectifyImageR, q;
	Rect validRoil, validRoir;

	Size imageSize = leftImage.size();
	stereoRectify(leftIntrinsics, discoff, rightIntrinsics, discoff, imageSize,
						rotationLeftInRight, translationLeftInRight, rL, rR, pL, pR, q, CALIB_ZERO_DISPARITY, -1, imageSize, &validRoil, &validRoir);

	initUndistortRectifyMap(leftIntrinsics, discoff, rL, pL, imageSize, CV_32FC1, mapLx, mapLy);
	initUndistortRectifyMap(rightIntrinsics, discoff, rR, pR, imageSize, CV_32FC1, mapRx, mapRy);

	remap(leftImage, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
	remap(rightImage, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
#endif

	Elas::parameters param;
	param.disp_min = 0;
	param.disp_max = 256;
	param.support_threshold = 0.85f;
	param.support_texture = 10;
	param.candidate_stepsize = 5;
	param.incon_window_size = 5;
	param.incon_threshold = 5;
	param.incon_min_support = 5;
	param.add_corners = true;
	param.grid_size = 20;
	param.beta = 0.02;
	param.gamma = 3;
	param.sigma = 1;
	param.sradius = 3;
	param.match_texture = 1;
	param.lr_threshold = 1;
	param.speckle_sim_threshold = 1;
	param.speckle_size = 200;
	param.ipol_gap_width = 3;
	param.filter_median = false;
	param.filter_adaptive_mean = true;
	param.postprocess_only_left = true;
	param.subsampling = false;

	int64 t = getTickCount();
	Elas elas(param);
	elas.process(leftImage.data, rightImage.data, dispLeft.ptr<float>(0), dispRight.ptr<float>(0), dim);

	t = getTickCount() - t;
    printf("Time elapsed: %fms\n", t*1000/getTickFrequency());

	for(int i=0; i<height; i++)
	{
		for(int j=0; j<width; j++)
		{
			uchar depth = dispLeft.at<uchar>(i,j);
			if(depth <= 0.0f)
			{
				dispLeft.at<uchar>(i,j) = 255;
			}
			else
			{
				//z = f*b/(xl-xr)
				dispLeft.at<uchar>(i,j) = depthFact /depth;
			}
			printf("%u\t", dispLeft.at<uchar>(i,j));
		}
		printf("\n");
	}

	dispLeft = dispLeft * 30;

	imwrite("./testImage/elas_disp_left.jpg", dispLeft);
}



